/**
 * @file Visualization.cpp
 * @brief 可视化实现 - 修复显示问题版本
 */

#include "core/Visualization.h"
#include <iostream>
#include <sstream>
#include <iomanip>

namespace PathPlanning
{
Visualizer::Visualizer(std::shared_ptr<Map> map) : map_(map) {}

Visualizer::~Visualizer()
{
    cleanup();
}

bool Visualizer::initialize(int windowWidth, int windowHeight)
{
    windowWidth_  = windowWidth;
    windowHeight_ = windowHeight;

    // 创建显示图像
    displayImage_ = cv::Mat::zeros(windowHeight, windowWidth, CV_8UC3);

    // 计算缩放比例
    scaleX_ = static_cast<double>(windowWidth) / map_->getWidth();
    scaleY_ = static_cast<double>(windowHeight) / map_->getHeight();

    // 创建窗口
    cv::namedWindow(windowName_, cv::WINDOW_AUTOSIZE);

    // 绘制地图
    drawMap();

    std::cout << "Visualizer initialized" << std::endl;
    return true;
}

void Visualizer::displayResult(const PlanningResult& result, const Point& start, const Point& goal,
                               const std::string& plannerName)
{
    // 清空显示，重新绘制地图
    drawMap();

    // 绘制探索节点（如果存在）
    if (!result.exploredNodes.empty())
    {
        drawNodes(result.exploredNodes, cv::Scalar(200, 200, 100), 1);
    }

    // 绘制路径（如果存在）
    if (!result.path.empty())
    {
        drawPath(result.path, cv::Scalar(0, 0, 255), 3);
    }

    // 绘制起点和终点
    drawStartGoal(start, goal);

    // 绘制文本信息
    drawTextInfo(result, plannerName);

    // 显示图像
    cv::imshow(windowName_, displayImage_);

    // 强制刷新显示
    cv::waitKey(1);
}

void Visualizer::updateDisplay(const PlanningResult& partialResult, const Point& start, const Point& goal,
                               const std::string& plannerName)
{
    // 与displayResult相同，但用于实时更新
    displayResult(partialResult, start, goal, plannerName);
}

void Visualizer::waitKey(int delay)
{
    cv::waitKey(delay);
}

void Visualizer::setMouseCallback(std::function<void(int, int, int, int, void*)> callback)
{
    cv::setMouseCallback(
        windowName_,
        [](int event, int x, int y, int flags, void* userdata) {
            auto callbackPtr = static_cast<std::function<void(int, int, int, int, void*)>*>(userdata);
            if (callbackPtr && *callbackPtr)
            {
                (*callbackPtr)(event, x, y, flags, userdata);
            }
        },
        &callback);
}

cv::Point Visualizer::worldToImage(const Point& worldPoint) const
{
    GridCell gridCell = map_->worldToGrid(worldPoint);
    return cv::Point(static_cast<int>(gridCell.x * scaleX_), static_cast<int>(gridCell.y * scaleY_));
}

Point Visualizer::imageToWorld(const cv::Point& imagePoint) const
{
    int gridX = static_cast<int>(imagePoint.x / scaleX_);
    int gridY = static_cast<int>(imagePoint.y / scaleY_);
    return map_->gridToWorld(gridX, gridY);
}

void Visualizer::cleanup()
{
    if (!windowName_.empty())
    {
        cv::destroyWindow(windowName_);
    }
}

void Visualizer::drawMap()
{
    const auto& mapData = map_->getMapData();
    int         width   = map_->getWidth();
    int         height  = map_->getHeight();

    // 填充背景为白色
    displayImage_.setTo(cv::Scalar(255, 255, 255));

    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            uint8_t value = mapData[y * width + x];

            // 只绘制障碍物，自由空间保持白色
            if (value > 128)
            { // 障碍物
                cv::rectangle(displayImage_, cv::Point(static_cast<int>(x * scaleX_), static_cast<int>(y * scaleY_)),
                              cv::Point(static_cast<int>((x + 1) * scaleX_), static_cast<int>((y + 1) * scaleY_)),
                              cv::Scalar(0, 0, 0), -1); // 黑色填充
            }
        }
    }
}

void Visualizer::drawPath(const std::vector<Pose>& path, const cv::Scalar& color, int thickness)
{
    if (path.size() < 2)
        return;

    // 绘制路径线段
    for (size_t i = 1; i < path.size(); ++i)
    {
        cv::Point p1 = worldToImage(path[i - 1].toPoint());
        cv::Point p2 = worldToImage(path[i].toPoint());
        cv::line(displayImage_, p1, p2, color, thickness);
    }

    // 绘制路径点
    for (const auto& pose : path)
    {
        cv::Point center = worldToImage(pose.toPoint());
        cv::circle(displayImage_, center, thickness + 1, color, -1);
    }
}

void Visualizer::drawNodes(const std::vector<Pose>& nodes, const cv::Scalar& color, int radius)
{
    for (const auto& node : nodes)
    {
        cv::Point center = worldToImage(node.toPoint());
        cv::circle(displayImage_, center, radius, color, -1);
    }
}

void Visualizer::drawStartGoal(const Point& start, const Point& goal)
{
    // 绘制起点 (绿色)
    if (start.x != 0 || start.y != 0)
    {
        cv::Point startPoint = worldToImage(start);
        cv::circle(displayImage_, startPoint, 8, cv::Scalar(0, 255, 0), -1);
        cv::putText(displayImage_, "Start", cv::Point(startPoint.x + 10, startPoint.y - 10), cv::FONT_HERSHEY_SIMPLEX,
                    0.5, cv::Scalar(0, 255, 0), 1);
    }

    // 绘制终点 (红色)
    if (goal.x != 0 || goal.y != 0)
    {
        cv::Point goalPoint = worldToImage(goal);
        cv::circle(displayImage_, goalPoint, 8, cv::Scalar(0, 0, 255), -1);
        cv::putText(displayImage_, "Goal", cv::Point(goalPoint.x + 10, goalPoint.y - 10), cv::FONT_HERSHEY_SIMPLEX, 0.5,
                    cv::Scalar(0, 0, 255), 1);
    }
}

void Visualizer::drawTextInfo(const PlanningResult& result, const std::string& plannerName)
{
    std::ostringstream info;
    info << "Planner: " << plannerName;

    if (result.success)
    {
        info << " | Success: Yes";
        info << " | Length: " << std::fixed << std::setprecision(2) << result.pathLength << "m";
    }
    else
    {
        info << " | Success: No";
    }

    info << " | Time: " << std::fixed << std::setprecision(3) << result.planningTime << "s";
    info << " | Iterations: " << result.iterations;

    if (!result.message.empty())
    {
        info << " | " << result.message;
    }

    // 在图像底部绘制信息
    cv::putText(displayImage_, info.str(), cv::Point(10, displayImage_.rows - 10), cv::FONT_HERSHEY_SIMPLEX, 0.5,
                cv::Scalar(0, 0, 0), 1);
}

} // namespace PathPlanning